from turtle import TurtleGraphicsError

from VolumeCalculator import VolumeCalculator
from BoneStorageModel import  BoneStorageModel
from PointGenerator import PointGenerator
from DisparityCutGenerator import DisparityCutGenerator
from ImageCorrector import ImageCorrector
from Camera import Camera
from DisparityGenerator import DisparityGenerator
import numpy as np
import open3d as o3d
import cv2
def test():
    file_path = "./相机信息/out.xls"
    imageCorrector = ImageCorrector("images/IMG_0028.jpg", file_path)
    depthGenerator = DisparityGenerator(imageCorrector)
    disparity,img_color = depthGenerator.generate(5,15,False)
    disparityCutGenerator = DisparityCutGenerator()
    disparity28_filtered = disparityCutGenerator.ParallaxSegmentation()
    pointGenerator = PointGenerator(disparity28_filtered,img_color)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(pointGenerator.points)
    pcd.colors = o3d.utility.Vector3dVector(pointGenerator.colors)
    """打开显示"""
    # o3d.visualization.draw_geometries([pcd])
    coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])
    """
    左边是红色
    中间是绿色
    右边是蓝色
    """
    rotation_angles = (np.radians(28), np.radians(18), np.radians(12))
    width = 1.3
    depth = 1.6
    height = 3.8
    Rx = o3d.geometry.get_rotation_matrix_from_axis_angle((rotation_angles[0], 0, 0))
    Ry = o3d.geometry.get_rotation_matrix_from_axis_angle((0, rotation_angles[1], 0))
    Rz = o3d.geometry.get_rotation_matrix_from_axis_angle((0, 0, rotation_angles[2]))
    rotation_matrix = Rz @ Ry @ Rx

    boneStorageModel = BoneStorageModel(depth,width,height,rotation_matrix)
    wireframe_walls = boneStorageModel.create_wireframe([0, 0, 1],rotation_matrix)

    volumeCalculator = VolumeCalculator(pointGenerator.points,1.9,disparity28_filtered,pointGenerator.colors)
    volumeCalculator.calculate_Bulk_Volume_points(volumeCalculator.height_D, rotation_matrix)
    pass

    o3d.visualization.draw_geometries([pcd, coordinate_frame, wireframe_walls])#点云、坐标系、墙面可视化
if __name__ == "__main__":
    test()


